﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Geometry {

    public class Transform {
        public static float DEG2RAD = (float) Math.PI / 180.0f;

        private Matrix44 m, mInv;

        public Transform() {
            this.m = new Matrix44();
            this.mInv = new Matrix44();
        }

        public Transform(float[][] mat) {
            System.Diagnostics.Debug.Assert(mat.Length == 4);
            System.Diagnostics.Debug.Assert(mat[0].Length == 4);

            this.m = new Matrix44(
                mat[0][0], mat[0][1], mat[0][2], mat[0][3],
                mat[1][0], mat[1][1], mat[1][2], mat[1][3],
                mat[2][0], mat[2][1], mat[2][2], mat[2][3],
                mat[3][0], mat[3][1], mat[3][2], mat[3][3]);
            this.mInv = this.m.inverse();
        }

        public Transform(Matrix44 mat) {
            this.m = mat;
            this.mInv = mat.inverse();
        }

        public Transform(Matrix44 mat, Matrix44 matInv) {
            this.m = mat;
            this.mInv = matInv;
        }

        public static Point operator *(Transform tr, Point pin) {
            Point pout = new Point();
            transform(tr, pin, ref pout);
            return pout;
        }

        public static Vector operator *(Transform tr, Vector vin) {
            Vector vout = new Vector();
            transform(tr, vin, ref vout);
            return vout;
        }

        public static Normal operator *(Transform tr, Normal nin) {
            Normal nout = new Normal();
            transform(tr, nin, ref nout);
            return nout;
        }

        public static BBox operator *(Transform tr, BBox bin) {
            BBox bout = new BBox();
            transform(tr, bin, ref bout);
            return bout;
        }

        public static Transform operator *(Transform ta, Transform tb) {
            Matrix44 m = ta.m * tb.m;
            Matrix44 minv = tb.mInv * ta.mInv;
            return new Transform(m, minv);
        }

        public static void transform(Transform tr, Point pin, ref Point pout) {
            var x = pin.x; var y = pin.y; var z = pin.z;
            var m = tr.m.m;

            float px = m[0][0] * x + m[0][1] * y + m[0][2] * z + m[0][3];
            float py = m[1][0] * x + m[1][1] * y + m[1][2] * z + m[1][3];
            float pz = m[2][0] * x + m[2][1] * y + m[2][2] * z + m[2][3];
            float pw = m[3][0] * x + m[3][1] * y + m[3][2] * z + m[3][3];

            if (pw != 0) {
                float invw = 1f / pw;
                px *= invw;
                py *= invw;
                pz *= invw;
            }
            pout.x = px;
            pout.y = py;
            pout.z = pz;
        }

        public static void transform(Transform tr, Vector vin, ref Vector vout) {
            var x = vin.x; var y = vin.y; var z = vin.z;
            var m = tr.m.m;

            float px = m[0][0] * x + m[0][1] * y + m[0][2] * z;
            float py = m[1][0] * x + m[1][1] * y + m[1][2] * z;
            float pz = m[2][0] * x + m[2][1] * y + m[2][2] * z;

            vout.x = px;
            vout.y = py;
            vout.z = pz;
        }

        public static void transform(Transform tr, Normal nin, ref Normal nout) {
            var x = nin.x; var y = nin.y; var z = nin.z;
            var m = tr.mInv.m;

            float px = m[0][0] * x + m[0][1] * y + m[0][2] * z;
            float py = m[1][0] * x + m[1][1] * y + m[1][2] * z;
            float pz = m[2][0] * x + m[2][1] * y + m[2][2] * z;

            nout.x = px;
            nout.y = py;
            nout.z = pz;
        }

        public static void transform(Transform tr, Ray rin, ref Ray rout) {
            transform(tr, rin.o, ref rout.o);
            transform(tr, rin.d, ref rout.d);
            rout.mint = rin.mint;
            rout.maxt = rin.maxt;
            rout.time = rin.time;
        }

        public static void transform(Transform tr, BBox bin, ref BBox bout) {
            Point pm = tr * bin.pMin;
            Vector vm = bin.pMax - bin.pMin;
            Matrix44 md = tr.m * new Matrix44(
                vm.x, 0, 0, 0,
                0, vm.y, 0, 0,
                0, 0, vm.z, 0,
                0, 0, 0, 0);
            
            Vector vpos = new Vector();
            Vector vneg = new Vector();
            var m = md.m;
            for (int col = 0; col < 3; ++col) {
                var dx = m[0][col];
                var dy = m[1][col];
                var dz = m[2][col];
                if (dx > 0)
                    vpos.x += dx;
                else
                    vneg.x += dx;
                if (dy > 0)
                    vpos.y += dy;
                else
                    vneg.y += dy;
                if (dz > 0)
                    vpos.z += dz;
                else
                    vneg.z += dz;
            }

            bout.pMin = pm + vneg;
            bout.pMax = pm + vpos;
        }

        public Transform getInverse() {
            return new Transform(this.mInv, this.m);
        }

        public static Transform translate(Vector delta) {
            Matrix44 m, mInv;
            m = new Matrix44(
                1, 0, 0, delta.x,
                0, 1, 0, delta.y,
                0, 0, 1, delta.z,
                0, 0, 0, 1);
            mInv = new Matrix44(
                1, 0, 0, -delta.x,
                0, 1, 0, -delta.y,
                0, 0, 1, -delta.z,
                0, 0, 0, 1);
            return new Transform(m, mInv);
        }

        public static Transform scale(float x, float y, float z) {
            Matrix44 m, minv;
            m = new Matrix44(
                x, 0, 0, 0,
                0, y, 0, 0,
                0, 0, z, 0,
                0, 0, 0, 1);
            minv = new Matrix44(
                1.0f / x, 0, 0, 0,
                0, 1.0f / y, 0, 0,
                0, 0, 1.0f / z, 0,
                0, 0, 0, 1);
            return new Transform(m, minv);
        }

        public static Transform rotateX(float angle) {
            float sint = (float) Math.Sin(DEG2RAD * angle);
            float cost = (float) Math.Cos(DEG2RAD * angle);

            Matrix44 m = new Matrix44(
                1, 0, 0, 0,
                0, cost, -sint, 0,
                0, sint, cost, 0,
                0, 0, 0, 1);
            return new Transform(m, m.inverse());
        }

        public static Transform rotateY(float angle) {
            float sint = (float)Math.Sin(DEG2RAD * angle);
            float cost = (float)Math.Cos(DEG2RAD * angle);

            Matrix44 m = new Matrix44(
                cost, 0, sint, 0,
                0, 1, 0, 0,
                -sint, 0, cost, 0,
                0, 0, 0, 1);
            return new Transform(m, m.inverse());
        }

        public static Transform rotateZ(float angle) {
            float sint = (float)Math.Sin(DEG2RAD * angle);
            float cost = (float)Math.Cos(DEG2RAD * angle);

            Matrix44 m = new Matrix44(
                cost, -sint, 0, 0,
                sint, cost, 0, 0,
                0, 0, 1, 0,
                0, 0, 0, 1);
            return new Transform(m, m.inverse());
        }

        public static Transform LookAt(Point pos, Point look, Vector up) {
            Matrix44 cam2world;

            Vector forward = (look - pos).normalize();
            Vector right = up.cross(forward).normalize();
            Vector orthUp = forward.cross(right);

            cam2world = new Matrix44(
                right.x, orthUp.x, forward.x, pos.x,
                right.y, orthUp.y, forward.y, pos.y,
                right.z, orthUp.z, forward.z, pos.z,
                0, 0, 0, 1);
            return new Transform(cam2world.inverse(), cam2world);
        }
    }


}
